/*
 * Copyright (c) 2021, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "Control.h"
#include "Delay.h"
#include "Encoder.h"
#include "I2C.h"
#include "IMU.h"
#include "MPU6050.h"
#include "Motor.h"
#include "OLED.h"
#include "PID.h"
#include "Sensor.h"
#include "atk_ms901m.h"
#include "atk_ms901m_uart.h"

// #include "bsp_mpu6050.h"
// #include "inv_mpu.h"

#include "UART.h"
#include "ti_msp_dl_config.h"
#include <stdio.h>
#include <string.h>

void Float_to_Byte(float f, uint8_t *byte);

#define USART_TX_LEN 11             // 数据包大小
uint8_t USART_TX_BUF[USART_TX_LEN]; // 数据包缓存区

int main(void) {
  SYSCFG_DL_init();

  // 定时器初始化
  DL_TimerG_startCounter(TIMER_0_INST);
  NVIC_EnableIRQ(TIMER_0_INST_INT_IRQN);

  UART_Init();
  OLED_Init();
  Encoder_Init();
  Motor_Init();
  Sensor_PID_Param_Init();
  //   PID_Param_Init();
  //-- ------------ MPU6050 初始化部分 ---------------------
  atk_ms901m_uart_init(1);
  //   MPU6050_Init();

  Yaw_Init();

  OLED_Static_Show();
 

  while (1) {
    Conditional_Judgment();
    OLED_Loop_Show();

    // ---------------------- TEST --------------------------------

    // ------------------------- END -------------------------------

    // IMU_getEuleranAngles();
    // MPU6050_GetData(&x, &y, &z, &x1, &y1, &z1);
    // OLED_ShowSignedNum(4, 1, imu_Angle.Pitch, 3);
    // OLED_ShowSignedNum(4, 1, z1, 5);
    // OLED_ShowSignedNum(4, 7, MPU6050_Temp(), 5);
    // OLED_ShowSignedNum(4, 13, imu_Angle.Yaw, 3);
    // delay_ms(5);

    // ------------------------------- UART -----------------------------------

    // USART_TX_BUF[0] = 0xA5;

    // Float_to_Byte(APWM, &USART_TX_BUF[1]);
    // Float_to_Byte(BPWM, &USART_TX_BUF[5]);

    // // 计算校验和
    // USART_TX_BUF[9] =
    //     (uint8_t)((USART_TX_BUF[1] + USART_TX_BUF[2] + USART_TX_BUF[3] +
    //                USART_TX_BUF[4] + USART_TX_BUF[5] + USART_TX_BUF[6] +
    //                USART_TX_BUF[7] + USART_TX_BUF[8]) &
    //               0xff);

    // USART_TX_BUF[10] = 0x5A;

    // // 通过串口1发送
    // UART_SendArray(USART_TX_BUF, USART_TX_LEN, UART_0_INST);

    switch (UART_PID) {
    case 0x01:
      Motor1_PID.Kp += 0.01;
      Motor2_PID.Kp += 0.01;
      HCSR04_PID.Kp += 0.01;
      Sensor_PID.Kp += 0.01;
      MPU6050_PID.Kp += 0.01;
      break;
    case 0x02:
      Motor1_PID.Kp += 0.1;
      Motor2_PID.Kp += 0.1;
      HCSR04_PID.Kp += 0.1;
      Sensor_PID.Kp += 0.1;
      MPU6050_PID.Kp += 0.1;
      break;
    case 0x03:
      Motor1_PID.Kp += 1;
      Motor2_PID.Kp += 1;
      HCSR04_PID.Kp += 1;
      Sensor_PID.Kp += 1;
      MPU6050_PID.Kp += 1;
      break;
    case 0x04:
      Motor1_PID.Kp += 10;
      Motor2_PID.Kp += 10;
      HCSR04_PID.Kp += 10;
      Sensor_PID.Kp += 10;
      MPU6050_PID.Kp += 10;
      break;
    case 0x05:
      Motor1_PID.Kd += 0.01;
      Motor2_PID.Kd += 0.01;
      HCSR04_PID.Kd += 0.01;
      Sensor_PID.Kd += 0.01;
      MPU6050_PID.Kd += 0.01;
      break;
    case 0x06:
      Motor1_PID.Kd += 0.1;
      Motor2_PID.Kd += 0.1;
      HCSR04_PID.Kd += 0.1;
      Sensor_PID.Kd += 0.1;
      MPU6050_PID.Kd += 0.1;
      break;
    case 0x07:
      Motor1_PID.Kd += 1;
      Motor2_PID.Kd += 1;
      HCSR04_PID.Kd += 1;
      Sensor_PID.Kd += 1;
      MPU6050_PID.Kd += 1;
      break;
    case 0x08:
      Motor1_PID.Kd += 10;
      Motor2_PID.Kd += 10;
      HCSR04_PID.Kd += 10;
      Sensor_PID.Kd += 10;
      MPU6050_PID.Kd += 10;
      break;
    case 0x09:
      Motor1_PID.Ki += 0.01;
      Motor2_PID.Ki += 0.01;
      HCSR04_PID.Kd += 0.01;
      Sensor_PID.Kd += 0.01;
      MPU6050_PID.Ki += 0.01;
      break;
    case 0x0A:
      Motor1_PID.Ki += 0.1;
      Motor2_PID.Ki += 0.1;
      HCSR04_PID.Kd += 0.1;
      Sensor_PID.Kd += 0.1;
      MPU6050_PID.Ki += 0.1;
      break;
    case 0x0B:
      Motor1_PID.Ki += 1;
      Motor2_PID.Ki += 1;
      HCSR04_PID.Kd += 1;
      Sensor_PID.Kd += 1;
      MPU6050_PID.Ki += 1;
      break;
    case 0x0C:
      Motor1_PID.Ki += 10;
      Motor2_PID.Ki += 10;
      HCSR04_PID.Kd += 10;
      Sensor_PID.Kd += 10;
      MPU6050_PID.Ki += 10;
      break;
    case 0x0E:
      Motor1_PID.target_val = 150;
      Motor2_PID.target_val = 150;
      //   MPU6050_PID.target_val += 90;
      break;
    case 0x0F:
      Motor1_PID.target_val = 30;
      Motor2_PID.target_val = 30;
      //   MPU6050_PID.target_val -= 90;
      break;
    }

    UART_PID = 0;
  }
}

void Float_to_Byte(float f, uint8_t *byte) {

  unsigned long longdata = 0;
  longdata = *(unsigned long *)&f;
  byte[3] = (longdata & 0xFF000000) >> 24;
  byte[2] = (longdata & 0x00FF0000) >> 16;
  byte[1] = (longdata & 0x0000FF00) >> 8;
  byte[0] = (longdata & 0x000000FF);
}
